#!/usr/bin/env python3
#   Motor_Up_serial_number = 208334654230
#   Motor_Down_serial_number = 207A34634230
import odrive
import time
from odrive.enums import *

odrive_is_connected = 0
odrv_up = None
odrv_down = None
arm_calibrate_angles = [0.123, -0.308]
# hand_calibrate_angles = [-0.18, -0.23]
hand_calibrate_angles = [-0.37, -0.21]
def faster_Arms():
    # odrv_up.axis0.trap_traj.config.accel_limit = 2000
    # odrv_up.axis1.trap_traj.config.accel_limit = 2000
    # odrv_up.axis0.trap_traj.config.decel_limit = 2000
    # odrv_up.axis1.trap_traj.config.decel_limit = 2000
    odrv_up.axis0.trap_traj.config.accel_limit = 220
    odrv_up.axis1.trap_traj.config.accel_limit = 220
    odrv_up.axis0.trap_traj.config.decel_limit = 100
    odrv_up.axis1.trap_traj.config.decel_limit = 100
def slower_Arms():
    odrv_up.axis0.trap_traj.config.accel_limit = 220
    odrv_up.axis1.trap_traj.config.accel_limit = 220
    odrv_up.axis0.trap_traj.config.decel_limit = 100
    odrv_up.axis1.trap_traj.config.decel_limit = 100
def odrive_connect():
    global odrv_up, odrv_down
    global odrive_is_connected
    try:
        print('Odrive start connecting')
        odrv_up = odrive.find_any(serial_number="208334654230") 
        odrv_down = odrive.find_any(serial_number="306739783435")
        slower_Arms()
        print('Odrive is connected!') 
    except Exception as error:
        print("Odrive Connection Error")
        print(error)
odrive_connect()
def odrive_up_calibrate():
    print('Robot arm start calibrating')
    odrv_up.axis0.requested_state = AXIS_STATE_ENCODER_INDEX_SEARCH
    while odrv_up.axis0.current_state != 1:
        time.sleep(0.1)
    odrv_up.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
    time.sleep(1)
    odrv_up.axis0.controller.input_pos = arm_calibrate_angles[0]
    time.sleep(1)
    odrv_up.axis1.requested_state = AXIS_STATE_ENCODER_INDEX_SEARCH
    while odrv_up.axis1.current_state != 1:
        time.sleep(0.1)
    odrv_up.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
    time.sleep(1)
    odrv_up.axis1.controller.input_pos = arm_calibrate_angles[1]
    print('Robot arm is calibrated!')

def odrive_down_calibrate():
    print('Robot hand start calibrating')
    odrv_down.axis0.requested_state = AXIS_STATE_ENCODER_INDEX_SEARCH
    while odrv_down.axis0.current_state != 1:
        time.sleep(0.1)
    odrv_down.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
    time.sleep(1)
    odrv_down.axis0.controller.input_pos = hand_calibrate_angles[0]
    time.sleep(1)
    odrv_down.axis1.requested_state = AXIS_STATE_ENCODER_INDEX_SEARCH
    while odrv_down.axis1.current_state != 1:
        time.sleep(0.1)
    odrv_down.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
    time.sleep(1)
    odrv_down.axis1.controller.input_pos = hand_calibrate_angles[1]
    print('Robot hand is calibrated!')

def arm0_rotate90_clockwise():
    odrv_up.axis0.controller.move_incremental(0.5,True)
def arm0_rotate180_clockwise():
    odrv_up.axis0.controller.move_incremental(1,True)    
def arm0_rotate90_anticlockwise():
    odrv_up.axis0.controller.move_incremental(-0.5,True)  

def arm1_rotate90_clockwise():
    odrv_up.axis1.controller.move_incremental(0.5,True)
def arm1_rotate180_clockwise():
    odrv_up.axis1.controller.move_incremental(1,True)    
def arm1_rotate90_anticlockwise():
    odrv_up.axis1.controller.move_incremental(-0.5,True) 

def catch_hand0():
    odrv_down.axis0.controller.input_pos = -0.52
def loosen_hand0():
    odrv_down.axis0.controller.input_pos = 0.1
def catch_hand1():
    odrv_down.axis1.controller.input_pos = -0.31
def loosen_hand1():
    odrv_down.axis1.controller.input_pos = 0.29

def wait_cube_hand():
    odrv_down.axis0.controller.input_pos = hand_calibrate_angles[0]
    odrv_down.axis1.controller.input_pos = hand_calibrate_angles[1]
# def returnToInitial():

def Robot_Init():
    # odrive_connect()
    odrive_down_calibrate()
    odrive_up_calibrate()
    # wait_cube_hand()

